//
// Created by Administrator on 2021/11/1.
//

#ifndef RASTERIZATION_STUDY_GEOMETRY_HPP
#define RASTERIZATION_STUDY_GEOMETRY_HPP

#include <iostream>
#include <iomanip>

constexpr float PI = 3.1415f;
constexpr float PI2 = 6.2830f;
constexpr float Deg2Rad = PI / 180.0f;
constexpr float Rad2Deg = 180.0f / PI;

template<typename T>
class Vec2
{
public:
    Vec2() : m_X(0), m_Y(0) {}
    explicit Vec2(T v) : m_X(v), m_Y(v) {}
    Vec2(T x, T y) : m_X(x), m_Y(y) {}

    Vec2 operator + (const Vec2& rhs) const
    {
        return Vec2(m_X + rhs.x(), m_Y + rhs.y());
    }
    Vec2 operator - (const Vec2& rhs) const
    {
        return Vec2(m_X - rhs.x(), m_Y - rhs.y());
    }
    Vec2 operator * (const T& rhs) const
    {
        return Vec2(m_X * rhs, m_Y * rhs);
    }
    Vec2 operator / (const T& rhs) const
    {
        return Vec2(m_X / rhs, m_Y / rhs);
    }
    Vec2& operator += (const Vec2& rhs)
    {
        m_X += rhs.x();
        m_Y += rhs.y();
        return *this;
    }
    Vec2& operator -= (const Vec2& rhs)
    {
        m_X -= rhs.x();
        m_Y -= rhs.y();
        return *this;
    }
    Vec2& operator *= (const Vec2& rhs)
    {
        m_X *= rhs.x();
        m_Y *= rhs.y();
        return *this;
    }
    Vec2& operator /= (const Vec2& rhs)
    {
        m_X /= rhs.x();
        m_Y /= rhs.y();
        return *this;
    }
    friend Vec2 operator * (const T& lhs, const Vec2<T>& rhs)
    {
        return Vec2(lhs * rhs.x(), lhs * rhs.y());
    }
    friend std::ostream& operator << (std::ostream& o, const Vec2<T> &rhs)
    {
        return o << "[" << rhs.x() << " " << rhs.y() << "]";
    }

    const T& x() const { return m_X; }
    T& x() { return m_X; }
    const T& y() const { return m_Y; }
    T& y() { return m_Y; }

private:
    T m_X;
    T m_Y;
};

using Vec2f = Vec2<float>;
using Vec2i = Vec2<int>;

template<typename T>
class Vec3
{
public:
    Vec3() : m_X(0), m_Y(0), m_Z(0) {}
    explicit Vec3(T v) : m_X(v), m_Y(v), m_Z(v) {}
    Vec3(T x, T y, T z) : m_X(x), m_Y(y), m_Z(z) {}

    T Dot(const Vec3& rhs) const
    {
        return m_X * rhs.x() + m_Y * rhs.y() + m_Z * rhs.z();
    }

    Vec3 Cross(const Vec3& rhs) const
    {
        return Vec3(m_Y * rhs.z() - m_Z * rhs.y(),
                    m_Z * rhs.x() - m_X * rhs.z(),
                    m_X * rhs.y() - m_Y * rhs.x());
    }

    T Length2() const
    {
        return m_X * m_X + m_Y * m_Y + m_Z * m_Z;
    }

    T Length() const
    {
        return std::sqrt(Length2());
    }

    Vec3 Norm() const
    {
        Vec3 result = *this;
        result.Normalize();
        return result;
    }

    Vec3& Normalize()
    {
        T len2 = Length2();
        if(len2 > 0)
        {
            T invLen = 1 / std::sqrt(len2);
            m_X *= invLen;
            m_Y *= invLen;
            m_Z *= invLen;
        }

        return *this;
    }

    Vec3 operator + (const Vec3& rhs) const
    {
        return Vec3(m_X + rhs.x(), m_Y + rhs.y(), m_Z + rhs.z());
    }
    Vec3 operator - (const Vec3& rhs) const
    {
        return Vec3(m_X - rhs.x(), m_Y - rhs.y(), m_Z - rhs.z());
    }
    Vec3 operator * (const T& rhs) const
    {
        return Vec3(m_X * rhs, m_Y * rhs, m_Z * rhs);
    }
    Vec3 operator / (const T& rhs) const
    {
        return Vec3(m_X / rhs, m_Y / rhs, m_Z / rhs);
    }
    Vec3 operator - () const
    {
        return Vec3(-m_X, -m_Y, -m_Z);
    }
    Vec3& operator += (const Vec3& rhs)
    {
        m_X += rhs.x();
        m_Y += rhs.y();
        m_Z += rhs.z();
        return *this;
    }
    Vec3& operator -= (const Vec3& rhs)
    {
        m_X -= rhs.x();
        m_Y -= rhs.y();
        m_Z -= rhs.z();
        return *this;
    }
    Vec3& operator *= (const T& rhs)
    {
        m_X *= rhs;
        m_Y *= rhs;
        m_Z *= rhs;
        return *this;
    }
    Vec3& operator /= (const T& rhs)
    {
        m_X /= rhs;
        m_Y /= rhs;
        m_Z /= rhs;
        return *this;
    }
    const T& operator [] (uint8_t i) const { return (&m_X)[i]; }
    T& operator [] (uint8_t i) { return (&m_X)[i]; }
    friend Vec3 operator * (const T& lhs, const Vec3<T>& rhs)
    {
        return Vec3(lhs * rhs.x(), lhs * rhs.y(), lhs * rhs.z());
    }
    friend Vec3 operator / (const T& lhs, const Vec3<T>& rhs)
    {
        return Vec3(lhs / rhs.x(), lhs / rhs.y(), lhs / rhs.z());
    }
    friend std::ostream& operator << (std::ostream& o, const Vec3<T>& rhs)
    {
        return o << "[" << rhs.x() << " " << rhs.y() << " " << rhs.z() << "]";
    }

    const T& x() const { return m_X; }
    T& x() { return m_X; }
    const T& y() const { return m_Y; }
    T& y() { return m_Y;}
    const T& z() const { return m_Z; }
    T& z() { return m_Z; }

private:
    T m_X;
    T m_Y;
    T m_Z;
};

using Vec3f = Vec3<float>;
using Vec3i = Vec3<int>;

template<typename T>
class Mat4x4
{
public:
    Mat4x4()
    {
        memset(m_Data, 0, 16 * sizeof(float));
    }
    Mat4x4(T m00, T m01, T m02, T m03,
           T m10, T m11, T m12, T m13,
           T m20, T m21, T m22, T m23,
           T m30, T m31, T m32, T m33)
    {
        m_Data[0][0] = m00;
        m_Data[0][1] = m01;
        m_Data[0][2] = m02;
        m_Data[0][3] = m03;
        m_Data[1][0] = m10;
        m_Data[1][1] = m11;
        m_Data[1][2] = m12;
        m_Data[1][3] = m13;
        m_Data[2][0] = m20;
        m_Data[2][1] = m21;
        m_Data[2][2] = m22;
        m_Data[2][3] = m23;
        m_Data[3][0] = m30;
        m_Data[3][1] = m31;
        m_Data[3][2] = m32;
        m_Data[3][3] = m33;
    }

    Mat4x4 Transposed() const
    {
        return Mat4x4(
                m_Data[0][0], m_Data[1][0], m_Data[2][0], m_Data[3][0],
                m_Data[0][1], m_Data[1][1], m_Data[2][1], m_Data[3][1],
                m_Data[0][2], m_Data[1][2], m_Data[2][2], m_Data[3][2],
                m_Data[0][3], m_Data[1][3], m_Data[2][3], m_Data[3][3]
                );
    }

    Mat4x4& Transpose()
    {
        Mat4x4 tmp = Mat4x4(
                m_Data[0][0], m_Data[1][0], m_Data[2][0], m_Data[3][0],
                m_Data[0][1], m_Data[1][1], m_Data[2][1], m_Data[3][1],
                m_Data[0][2], m_Data[1][2], m_Data[2][2], m_Data[3][2],
                m_Data[0][3], m_Data[1][3], m_Data[2][3], m_Data[3][3]
        );

        *this = tmp;
        return *this;
    }

    void MulVec(const Vec3<T>& rhs, Vec3<T>& result) const
    {
        T x = m_Data[0][0] * rhs.x() + m_Data[0][1] * rhs.y() + m_Data[0][2] * rhs.z() + m_Data[0][3];
        T y = m_Data[1][0] * rhs.x() + m_Data[1][1] * rhs.y() + m_Data[1][2] * rhs.z() + m_Data[1][3];
        T z = m_Data[2][0] * rhs.x() + m_Data[2][1] * rhs.y() + m_Data[2][2] * rhs.z() + m_Data[2][3];
        T w = m_Data[3][0] * rhs.x() + m_Data[3][1] * rhs.y() + m_Data[3][2] * rhs.z() + m_Data[3][3];

        if(w != 1)
        {
            result.x() = x / w;
            result.y() = y / w;
            result.z() = z / w;
        }
        else
        {
            result.x() = x;
            result.y() = y;
            result.z() = z;
        }

    }

    void MulDir(const Vec3<T>& rhs, Vec3<T>& result) const
    {
        result.x() = m_Data[0][0] * rhs.x() + m_Data[0][1] * rhs.y() + m_Data[0][2] * rhs.z();
        result.y() = m_Data[1][0] * rhs.x() + m_Data[1][1] * rhs.y() + m_Data[1][2] * rhs.z();
        result.z() = m_Data[2][0] * rhs.x() + m_Data[2][1] * rhs.y() + m_Data[2][2] * rhs.z();
    }

    // Gauss-Jordan Algorithm
    Mat4x4 inverse() const
    {
        int i, j, k;
        Mat4x4 s = Identity();
        Mat4x4 t (*this);

        // Forward elimination
        for (i = 0; i < 3 ; i++) {
            int pivot = i;

            T pivotsize = t[i][i];

            if (pivotsize < 0)
                pivotsize = -pivotsize;

            for (j = i + 1; j < 4; j++) {
                T tmp = t[j][i];

                if (tmp < 0)
                    tmp = -tmp;

                if (tmp > pivotsize) {
                    pivot = j;
                    pivotsize = tmp;
                }
            }

            if (pivotsize == 0) {
                // Cannot invert singular matrix
                return Mat4x4();
            }

            if (pivot != i) {
                for (j = 0; j < 4; j++) {
                    T tmp;

                    tmp = t[i][j];
                    t[i][j] = t[pivot][j];
                    t[pivot][j] = tmp;

                    tmp = s[i][j];
                    s[i][j] = s[pivot][j];
                    s[pivot][j] = tmp;
                }
            }

            for (j = i + 1; j < 4; j++) {
                T f = t[j][i] / t[i][i];

                for (k = 0; k < 4; k++) {
                    t[j][k] -= f * t[i][k];
                    s[j][k] -= f * s[i][k];
                }
            }
        }

        // Backward substitution
        for (i = 3; i >= 0; --i) {
            T f;

            if ((f = t[i][i]) == 0) {
                // Cannot invert singular matrix
                return Mat4x4();
            }

            for (j = 0; j < 4; j++) {
                t[i][j] /= f;
                s[i][j] /= f;
            }

            for (j = 0; j < i; j++) {
                f = t[j][i];

                for (k = 0; k < 4; k++) {
                    t[j][k] -= f * t[i][k];
                    s[j][k] -= f * s[i][k];
                }
            }
        }

        return s;
    }

    // 伴随矩阵求逆
    Mat4x4 inverse_adj(T* determinant) const
    {
        // 先求转置
        Mat4x4 MT = Transposed();

        // A00 A01 A02 A03
        // A10 A11 A12 A13
        // A20 A21 A22 A23
        // A30 A31 A32 A33
        // 求代数余子式矩阵
        Mat4x4 MC;

        // A11 A12 A13
        // A21 A22 A23
        // A31 A32 A33
        MC[0][0] = MT[1][1] * (MT[2][2] * MT[3][3] - MT[2][3] * MT[3][2]) -
                MT[1][2] * (MT[2][1] * MT[3][3] - MT[2][3] * MT[3][1]) +
                MT[1][3] * (MT[2][1] * MT[3][2] - MT[2][2] * MT[3][1]);

        // A10 A12 A13
        // A20 A22 A23
        // A30 A32 A33
        MC[0][1] = -(MT[1][0] * (MT[2][2] * MT[3][3] - MT[2][3] * MT[3][2]) -
                MT[1][2] * (MT[2][0] * MT[3][3] - MT[2][3] * MT[3][0]) +
                MT[1][3] * (MT[2][0] * MT[3][2] - MT[2][2] * MT[3][0]));

        // A10 A11 A13
        // A20 A21 A23
        // A30 A31 A33
        MC[0][2] = MT[1][0] * (MT[2][1] * MT[3][3] - MT[2][3] * MT[3][1]) -
                MT[1][1] * (MT[2][0] * MT[3][3] - MT[2][3] * MT[3][0]) +
                MT[1][3] * (MT[2][0] * MT[3][1] - MT[2][1] * MT[3][0]);

        // A10 A11 A12
        // A20 A21 A22
        // A30 A31 A32
        MC[0][3] = -(MT[1][0] * (MT[2][1] * MT[3][2] - MT[2][2] * MT[3][1]) -
                MT[1][1] * (MT[2][0] * MT[3][2] - MT[2][2] * MT[3][0]) +
                MT[1][2] * (MT[2][0] * MT[3][1] - MT[2][1] * MT[3][0]));

        // A01 A02 A03
        // A21 A22 A23
        // A31 A32 A33
        MC[1][0] = -(MT[0][1] * (MT[2][2] * MT[3][3] - MT[2][3] * MT[3][2]) -
                MT[0][2] * (MT[2][1] * MT[3][3] - MT[2][3] * MT[3][1]) +
                MT[0][3] * (MT[2][1] * MT[3][2] - MT[2][2] * MT[3][1]));

        // A00 A02 A03
        // A20 A22 A23
        // A30 A32 A33
        MC[1][1] = MT[0][0] * (MT[2][2] * MT[3][3] - MT[2][3] * MT[3][2]) -
                MT[0][2] * (MT[2][0] * MT[3][3] - MT[2][3] * MT[3][0]) +
                MT[0][3] * (MT[2][0] * MT[3][2] - MT[2][2] * MT[3][0]);

        // A00 A01 A03
        // A20 A21 A23
        // A30 A31 A33
        MC[1][2] = -(MT[0][0] * (MT[2][1] * MT[3][3] - MT[2][3] * MT[3][1]) -
                MT[0][1] * (MT[2][0] * MT[3][3] - MT[2][3] * MT[3][0]) +
                MT[0][3] * (MT[2][0] * MT[3][1] - MT[2][1] * MT[3][0]));

        // A00 A01 A02
        // A20 A21 A22
        // A30 A31 A32
        MC[1][3] = MT[0][0] * (MT[2][1] * MT[3][2] - MT[2][2] * MT[3][1]) -
                MT[0][1] * (MT[2][0] * MT[3][2] - MT[2][2] * MT[3][0]) +
                MT[0][2] * (MT[2][0] * MT[3][1] - MT[2][1] * MT[3][0]);

        // A01 A02 A03
        // A11 A12 A13
        // A31 A32 A33
        MC[2][0] = MT[0][1] * (MT[1][2] * MT[3][3] - MT[1][3] * MT[3][2]) -
                   MT[0][2] * (MT[1][1] * MT[3][3] - MT[1][3] * MT[3][1]) +
                   MT[0][3] * (MT[1][1] * MT[3][2] - MT[1][2] * MT[3][1]);

        // A00 A02 A03
        // A10 A12 A13
        // A30 A32 A33
        MC[2][1] = -(MT[0][0] * (MT[1][2] * MT[3][3] - MT[1][3] * MT[3][2]) -
                   MT[0][2] * (MT[1][0] * MT[3][3] - MT[1][3] * MT[3][0]) +
                   MT[0][3] * (MT[1][0] * MT[3][2] - MT[1][2] * MT[3][0]));

        // A00 A01 A03
        // A10 A11 A13
        // A30 A31 A33
        MC[2][2] = MT[0][0] * (MT[1][1] * MT[3][3] - MT[1][3] * MT[3][1]) -
                   MT[0][1] * (MT[1][0] * MT[3][3] - MT[1][3] * MT[3][0]) +
                   MT[0][3] * (MT[1][0] * MT[3][1] - MT[1][1] * MT[3][0]);

        // A00 A01 A02
        // A10 A11 A12
        // A30 A31 A32
        MC[2][3] = -(MT[0][0] * (MT[1][1] * MT[3][2] - MT[1][2] * MT[3][1]) -
                   MT[0][1] * (MT[1][0] * MT[3][2] - MT[1][2] * MT[3][0]) +
                   MT[0][2] * (MT[1][0] * MT[3][1] - MT[1][1] * MT[3][0]));

        // A01 A02 A03
        // A11 A12 A13
        // A21 A22 A23
        MC[3][0] = -(MT[0][1] * (MT[1][2] * MT[2][3] - MT[1][3] * MT[2][2]) -
                   MT[0][2] * (MT[1][1] * MT[2][3] - MT[1][3] * MT[2][1]) +
                   MT[0][3] * (MT[1][1] * MT[2][2] - MT[1][2] * MT[2][1]));

        // A00 A02 A03
        // A10 A12 A13
        // A20 A22 A23
        MC[3][1] = MT[0][0] * (MT[1][2] * MT[2][3] - MT[1][3] * MT[2][2]) -
                   MT[0][2] * (MT[1][0] * MT[2][3] - MT[1][3] * MT[2][0]) +
                   MT[0][3] * (MT[1][0] * MT[2][2] - MT[1][2] * MT[2][0]);

        // A00 A01 A03
        // A10 A11 A13
        // A20 A21 A23
        MC[3][2] = -(MT[0][0] * (MT[1][1] * MT[2][3] - MT[1][3] * MT[2][1]) -
                   MT[0][1] * (MT[1][0] * MT[2][3] - MT[1][3] * MT[2][0]) +
                   MT[0][3] * (MT[1][0] * MT[2][1] - MT[1][1] * MT[2][0]));

        // A00 A01 A02
        // A10 A11 A12
        // A20 A21 A22
        MC[3][3] = MT[0][0] * (MT[1][1] * MT[2][2] - MT[1][2] * MT[2][1]) -
                   MT[0][1] * (MT[1][0] * MT[2][2] - MT[1][2] * MT[2][0]) +
                   MT[0][2] * (MT[1][0] * MT[2][1] - MT[1][1] * MT[2][0]);

        T d = m_Data[0][0] * MC[0][0] +
                  m_Data[0][1] * MC[1][0] +
                  m_Data[0][2] * MC[2][0] +
                  m_Data[0][3] * MC[3][0];

        // 计算行列式
        if(determinant != nullptr)
        {
            *determinant = d;
        }

        T invD = 1 / *determinant;

        return {
            MC[0][0] * invD, MC[0][1] * invD, MC[0][2] * invD, MC[0][3] * invD,
            MC[1][0] * invD, MC[1][1] * invD, MC[1][2] * invD, MC[1][3] * invD,
            MC[2][0] * invD, MC[2][1] * invD, MC[2][2] * invD, MC[2][3] * invD,
            MC[3][0] * invD, MC[3][1] * invD, MC[3][2] * invD, MC[3][3] * invD
        };
    }

    Mat4x4& invert()
    {
        *this = inverse();
        return *this;
    }

    static Mat4x4<T> Identity()
    {
        return Mat4x4<T>(1, 0, 0, 0,
                         0, 1, 0, 0,
                         0, 0, 1, 0,
                         0, 0, 0, 1);
    }

    static void Multiply(const Mat4x4<T>& lhs, const Mat4x4<T>& rhs, Mat4x4<T>& result)
    {
//        //Simple implements but low efficiency
//        for(uint8_t i = 0; i < 4; ++i)
//        {
//            for(uint8_t j = 0; j < 4; ++j)
//            {
//                result[i][j] = lhs[i][0] * rhs[0][j] +
//                               lhs[i][1] * rhs[1][j] +
//                               lhs[i][2] * rhs[2][j] +
//                               lhs[i][3] * rhs[3][j];
//            }
//        }

        const T* __restrict lp = &lhs[0][0];
        const T* __restrict rp = &rhs[0][0];
        T* __restrict sp = &result[0][0];

        T l0 = lp[0];
        T l1 = lp[1];
        T l2 = lp[2];
        T l3 = lp[3];

        sp[0] = l0 * rp[0] + l1 * rp[4] + l2 * rp[8] + l3 * rp[12];
        sp[1] = l0 * rp[1] + l1 * rp[5] + l2 * rp[9] + l3 * rp[13];
        sp[2] = l0 * rp[2] + l1 * rp[6] + l2 * rp[10] + l3 * rp[14];
        sp[3] = l0 * rp[3] + l1 * rp[7] + l2 * rp[11] + l3 * rp[15];

        l0 = lp[4];
        l1 = lp[5];
        l2 = lp[6];
        l3 = lp[7];

        sp[4] = l0 * rp[0] + l1 * rp[4] + l2 * rp[8] + l3 * rp[12];
        sp[5] = l0 * rp[1] + l1 * rp[5] + l2 * rp[9] + l3 * rp[13];
        sp[6] = l0 * rp[2] + l1 * rp[6] + l2 * rp[10] + l3 * rp[14];
        sp[7] = l0 * rp[3] + l1 * rp[7] + l2 * rp[11] + l3 * rp[15];

        l0 = lp[8];
        l1 = lp[9];
        l2 = lp[10];
        l3 = lp[11];

        sp[8] = l0 * rp[0] + l1 * rp[4] + l2 * rp[8] + l3 * rp[12];
        sp[9] = l0 * rp[1] + l1 * rp[5] + l2 * rp[9] + l3 * rp[13];
        sp[10] = l0 * rp[2] + l1 * rp[6] + l2 * rp[10] + l3 * rp[14];
        sp[11] = l0 * rp[3] + l1 * rp[7] + l2 * rp[11] + l3 * rp[15];

        l0 = lp[12];
        l1 = lp[13];
        l2 = lp[14];
        l3 = lp[15];

        sp[12] = l0 * rp[0] + l1 * rp[4] + l2 * rp[8] + l3 * rp[12];
        sp[13] = l0 * rp[1] + l1 * rp[5] + l2 * rp[9] + l3 * rp[13];
        sp[14] = l0 * rp[2] + l1 * rp[6] + l2 * rp[10] + l3 * rp[14];
        sp[15] = l0 * rp[3] + l1 * rp[7] + l2 * rp[11] + l3 * rp[15];
    }

    const T* operator [] (uint8_t i) const { return m_Data[i]; }
    T* operator [] (uint8_t i) { return m_Data[i]; }
    Mat4x4 operator * (const Mat4x4& rhs) const
    {
        Mat4x4 result;
        Multiply(*this, rhs, result);
        return result;
    }
    friend std::ostream& operator << (std::ostream& o, const Mat4x4<T> rhs)
    {
        std::ios_base::fmtflags oldFlags = o.flags();
        int width = 12;
        o.precision(5);
        o.setf(std::ios_base::fixed);

        o << "[" << std::setw(width) << rhs[0][0] <<
             " " << std::setw(width) << rhs[0][1] <<
             " " << std::setw(width) << rhs[0][2] <<
             " " << std::setw(width) << rhs[0][3] << "\n" <<
             " " << std::setw(width) << rhs[1][0] <<
             " " << std::setw(width) << rhs[1][1] <<
             " " << std::setw(width) << rhs[1][2] <<
             " " << std::setw(width) << rhs[1][3] << "\n" <<
             " " << std::setw(width) << rhs[2][0] <<
             " " << std::setw(width) << rhs[2][1] <<
             " " << std::setw(width) << rhs[2][2] <<
             " " << std::setw(width) << rhs[2][3] << "\n" <<
             " " << std::setw(width) << rhs[3][0] <<
             " " << std::setw(width) << rhs[3][1] <<
             " " << std::setw(width) << rhs[3][2] <<
             " " << std::setw(width) << rhs[3][3] << "]";

        o.flags(oldFlags);
        return o;
    }

    const T* data() const { return m_Data[0]; }
    T* data() { return m_Data[0]; }

private:
    T m_Data[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
};

using Mat4x4f = Mat4x4<float>;

#endif //RASTERIZATION_STUDY_GEOMETRY_HPP
